//ros头文件
#include <ros/ros.h>
#include <Eigen/Eigen>
#include <iostream>
//#include <boost/format.hpp>

//topic 头文件
#include <UAVcontrol_msgs/UAVState.h>
#include <UAVcontrol_msgs/UAVCommand.h>


using namespace std;

#define NODE_NAME "uav_terminal_control"
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>全 局 变 量<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
string uav_name;
int uav_id;

float state_desired[3];
//>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>主 函 数<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char **argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh("~");
    
    // 无人机编号 1号无人机则为1
    nh.param("uav_id", uav_id, 0);

    uav_name = "/uav" + std::to_string(uav_id);

    // 【发布】控制指令
    command_pub = nh.advertise<UAVcontrol_msgs::UAVCommand>(uav_name + "/prometheus/uav_command", 10);



    // Waiting for input
    int start_flag = 0;
    
    while (ros::ok()) 
    {
        cout << ">>>>>>>>>>>>>>>>>>>>>>>>>>>>UAV Terminal Control<<<<<<<<<<<<<<<<<<<<<<<<< "<< endl;
        cout << "Please choose the action: 0 for Hold, 1 for XYZ_VEL_BODY, 2 for XYZ_POS , 3 for XYZ_POS, 4 for Land "<<endl;
        cin >> start_flag;
    //无人机悬停控制模式  当前点悬停
        if (start_flag == 0)
        {
            uav_command.Agent_CMD = UAVcontrol_msgs::UAVCommand::Current_Pos_Hover;
            command_pub.publish(ugv_cmd);
            cout <<  " Hold mode "  << endl;
        }

    //无人机速度控制模式  body坐标系
        else if (start_flag == 1)
        {
            uav_command.Move_mode == UAVcontrol_msgs::UAVCommand::XYZ_VEL_BODY;
            cout << "desired state: --- linear_vel_x(body) [m/s] "<<endl;
            cin >> state_desired[0];
            cout << "desired state: --- linear_vel_y(body) [m/s] "<<endl;
            cin >> state_desired[1];
            cout << "desired state: --- yaw_vel(body) [deg/s]"<<endl;
            cin >> state_desired[2];

            uav_command.velocity_ref[0] = state_desired[0];
            uav_command.velocity_ref[1] = state_desired[1];
            uav_command.velocity_ref[2] = state_desired[2];
            acc_des << 0.0, 0.0, 0.0;
            yaw_rate_des = uav_command.yaw_rate_ref;
            yaw_des = uav_command.yaw_ref + uav_yaw;
 
            cout << "state_desired [XYZ_VEL_BODY] : " << state_desired[0] << " [m/s] "<< state_desired[1] << " [m/s] "<< state_desired[2] <<" [deg/s] "<< endl;

        } 

        //无人机位置控制模式  ENU坐标系
        else if (start_flag == 2)
        {


            uav_command.Move_mode = UAVcontrol_msgs::UAVCommand::XYZ_POS;

            cout << "desired state: --- position_x(ENU) [m] "<<endl;
            cin >> state_desired[0];
            cout << "desired state: --- position_y(ENU) [m] "<<endl;
            cin >> state_desired[1];
            cout << "desired state: --- position_z(ENU) [m]"<<endl;
            cin >> state_desired[2];


            uav_command.position_ref[0] = state_desired[0];
            uav_command.position_ref[1] = state_desired[1];
            uav_command.position_ref[2] = state_desired[2];
            pos_des[0] = uav_command.position_ref[0] - offset_pose.x;
            pos_des[1] = uav_command.position_ref[1] - offset_pose.y;
            pos_des[2] = uav_command.position_ref[2];
            vel_des << 0.0, 0.0, 0.0;
            acc_des << 0.0, 0.0, 0.0;
            yaw_des = uav_command.yaw_ref;
            cout << "state_desired [XYZ_POS] : " << state_desired[0] << " [m] "<< state_desired[1] << " [m] "<< state_desired[2] <<" [m] "<< endl;
        } 

        //无人机降落模式  Land mode
        else if (start_flag == 3)
        {

            uav_command.Agent_CMD = UAVcontrol_msgs::UAVCommand::Land;
            // 【Land】 降落，直接使用LAND_CONTROL
            control_state = CONTROL_STATE::LAND_CONTROL;

           cout << "state_desired [Land]:    " << endl;
        }
        else
        {
            cout << "Wrong input."<<endl;
        }
        ros::Duration(0.5).sleep();
    }
    return 0;

}